#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <termios.h>
#include <fcntl.h>
#include <unistd.h>
#include <iomanip>
#include <cstring>

using namespace std;

void print_header_check(const unsigned char* response, size_t len) {
    // Print the first two bytes in Hex format
    if (len >= 2) {
        cout << "First 2 bytes: ";
        for (int i = 0; i < 2; i++) {
            cout << hex << uppercase << setw(2) << setfill('0') << (int)response[i] << " ";
        }
        cout << "(Hex)" << endl;
        cout << "Response Length: " << len << endl;
    }
}

void parse_response(const unsigned char* response, size_t len) {
    // Check if response length is valid
    if (len < 27) {
        cout << "Invalid response length." << endl;
        return;
    }

    print_header_check(response, len);  // Print first two bytes for debugging

    // Header check (0xAA, 0x55)
    if (response[0] != 0xAA || response[1] != 0x55) {
        cout << "Invalid header." << endl;
        return;
    }

    // Get package length (2 bytes)
    unsigned int package_length = (response[2] << 8) | response[3];
    cout << "Package Length: " << package_length << " bytes" << endl;

    // Get package number (2 bytes)
    unsigned int package_no = (response[4] << 8) | response[5];
    cout << "Package No: " << package_no << endl;

    // Get the data part (24 bytes)
    unsigned char data[24];
    memcpy(data, response + 6, 24);
    cout << "Data (Hex): ";
    for (int i = 0; i < 24; i++) {
        cout << hex << uppercase << setw(2) << setfill('0') << (int)data[i] << " ";
    }
    cout << endl;

    // Convert each channel data (4 bytes per channel) to a float
    for (int i = 0; i < 6; i++) {
        float value;
        memcpy(&value, data + i * 4, 4);
        cout << "Channel " << i + 1 << " (Engineering Unit): " << fixed << setprecision(6) << value << " N (Force) / Nm (Torque)" << endl;
    }

    // Get the checksum (last byte)
    unsigned char checksum = response[len - 1];
    cout << "SUM Checksum: " << hex << uppercase << setw(2) << setfill('0') << (int)checksum << endl;
}

int main() {
    // Open serial port
    const char* port = "/dev/ttyUSB0";
    int serial_port = open(port, O_RDWR);

    if (serial_port < 0) {
        perror("Error opening serial port");
        return 1;
    }

    // Configure the serial port settings
    struct termios tty;
    memset(&tty, 0, sizeof(tty));

    if (tcgetattr(serial_port, &tty) != 0) {
        perror("Error getting terminal attributes");
        close(serial_port);
        return 1;
    }

    cfsetispeed(&tty, B115200);  // Set input baud rate
    cfsetospeed(&tty, B115200);  // Set output baud rate

    tty.c_cflag &= ~PARENB;    // Disable parity
    tty.c_cflag &= ~CSTOPB;    // One stop bit
    tty.c_cflag &= ~CSIZE;     // Clear size bits
    tty.c_cflag |= CS8;        // 8 data bits
    tty.c_cflag &= ~CRTSCTS;   // Disable hardware flow control
    tty.c_cflag |= CREAD | CLOCAL;  // Enable receiver, ignore modem control lines

    tty.c_iflag &= ~(IXON | IXOFF | IXANY);  // Disable software flow control
    tty.c_iflag &= ~(ICANON | ECHO | ECHOE | ISIG);  // Raw input mode
    tty.c_oflag &= ~OPOST;  // Raw output mode

    // Set new attributes
    if (tcsetattr(serial_port, TCSANOW, &tty) != 0) {
        perror("Error setting terminal attributes");
        close(serial_port);
        return 1;
    }

    unsigned char cmd[] = "AT+GOD\r\n";
    unsigned char response[100];  // Buffer for the response

    try {
        while (true) {
            // Flush the input buffer before reading new data
            tcflush(serial_port, TCIFLUSH);

            // Send command
            write(serial_port, cmd, sizeof(cmd) - 1);
            usleep(1000000);  // Wait for response (1 second)

            // Read the data frame fully (100 bytes in your case)
            int bytes_read = read(serial_port, response, sizeof(response));
            if (bytes_read > 0) {
                print_header_check(response, bytes_read);  // Print first 2 bytes for debugging

                // Check header
                if (response[0] == 0xAA && response[1] == 0x55) {
                    parse_response(response, bytes_read);  // Process valid frame
                } else {
                    cout << "Invalid header." << endl;
                }
            } else {
                cout << "No data received." << endl;
            }

            usleep(1000000);  // Wait before sending the next command
        }
    } catch (const exception& e) {
        cout << "Program interrupted: " << e.what() << endl;
    } catch (...) {
        cout << "Program interrupted." << endl;
    }

    close(serial_port);  // Close the serial port when done
    return 0;
}
